#include <stdio.h>
#include <string.h>

#include <thread>

// #include "state_machine/v1/state_machine.h"
// #include "state_machine/double_buffer/state_machine.h"

// #if HAS_BEHAVIOR_TREE
// #include "behavior_tree/behavior_tree.h"
// #endif

// int main()
// {
// #ifdef HAS_BEHAVIOR_TREE
//     testBehaviorTree();
// #endif

//     testStateMachine();

//     return 0;
// }

#include <iostream>

#include "state_machine/double_buffer/state_machine.h"

int main()
{
    init_tasks_interface();
    std::thread event_thread(event_processing_loop_interface);

    // 测试1: 发送低优先级事件（路径更新，可取消）
    // 预期: 当前任务（路径规划）优先级相同，会被暂停，新任务加入队列并执行
    std::cout << "\n===== 测试1: 发送路径更新事件（低优先级） =====" << std::endl;
    post_event_interface({Event::Type::PATH_UPDATE, Priority::LOW, "道路施工，需绕行"});
    std::this_thread::sleep_for(std::chrono::seconds(6));

    // 测试2: 再次发送路径更新事件（覆盖上一个可取消的路径更新任务）
    // 预期: 上一个路径更新任务（可取消）被移除，新的路径更新任务加入队列
    std::cout << "\n===== 测试2: 再次发送路径更新事件（覆盖可取消任务） =====" << std::endl;
    post_event_interface({Event::Type::PATH_UPDATE, Priority::LOW, "道路施工，新绕行方案"});
    std::this_thread::sleep_for(std::chrono::seconds(3));

    // // 测试3: 发送中优先级事件（障碍物检测，含两个可取消任务）
    // std::cout << "\n===== 测试3: 发送障碍物检测事件（中优先级） =====" << std::endl;
    // PostEventInterface({Event::Type::OBSTACLE_DETECTED, Priority::MEDIUM, "前方5米有障碍物"});

    // // 测试4: 发送高优先级事件（紧急停止，不可取消任务）

    // std::cout << "\n===== 测试4: 发送紧急停止事件（高优先级，不可取消） =====" << std::endl;
    // PostEventInterface({Event::Type::EMERGENCY_STOP, Priority::HIGH, "碰撞风险！"});

    // // 测试5: 再次发送路径更新事件（尝试覆盖不可取消任务）
    // std::cout << "\n===== 测试5: 发送路径更新事件（尝试覆盖不可取消任务） =====" << std::endl;
    // PostEventInterface({Event::Type::PATH_UPDATE, Priority::LOW, "紧急情况后新路径"});

    // // 测试6: 回滚任务（从紧急制动回滚到上一个任务）
    // std::cout << "\n===== 测试6: 执行任务回滚 =====" << std::endl;
    // RollbackTaskInterface();

    while (!all_tasks_processed)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    is_running = false;
    event_thread.join();

    return 0;
}
